#include "loopback.h"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <string>

using namespace cv;
using namespace std;

uint64_t time_ns() {
    return chrono::duration_cast<chrono::nanoseconds>(chrono::system_clock::now().time_since_epoch()).count();
}

void VideoCaptureLoopback::initializing(int width, int height, int fps) {
    this->width = width;
    this->height = height;
    this->fps = fps;
    keep_running = true;

    // if input type is camera, max_frame == -1
    max_frame = cam.get(CAP_PROP_FRAME_COUNT);
    cur_frame_count = 0;

    // Initialize camera
    cam.set(CAP_PROP_FRAME_WIDTH,  this->width);
    cam.set(CAP_PROP_FRAME_HEIGHT, this->height);
    cam.set(CAP_PROP_FPS, this->fps);
    cam.set(CAP_PROP_FOURCC, VideoWriter::fourcc('M', 'J', 'P', 'G'));

    // Initialize capturing thread
    cur_frame = Mat(this->height, this->width, CV_8UC3);
    capturing_thread = thread(&VideoCaptureLoopback::capturing, this);
}

VideoCaptureLoopback::VideoCaptureLoopback(int index, int virtual_index, int width, int height, int fps):
        virtual_cam(virtual_index, width, height), cam(index) {
    initializing(width, height, fps);
}

VideoCaptureLoopback::VideoCaptureLoopback(string index, int virtual_index, int width, int height, int fps):
        virtual_cam(virtual_index, width, height), cam(index) {
    initializing(width, height, fps);
}

VideoCaptureLoopback::~VideoCaptureLoopback() {
    this->release();
}

void VideoCaptureLoopback::capturing() {
    while(cam.isOpened() and keep_running) {
        uint64_t timer = time_ns();

        frame_mutex.lock();
        cam >> cur_frame;
        frame_mutex.unlock();

        virtual_cam.write_frame(cur_frame);

        cur_frame_count++;

        if(cur_frame_count == max_frame) {
            cam.set(CAP_PROP_POS_FRAMES, 0);
            cur_frame_count = 0;
        }

        while (time_ns() - timer < 10e8/fps);
    }
}

void VideoCaptureLoopback::read_frame(const _OutputArray &image) {
    frame_mutex.lock();
    image.assign(cur_frame);
    frame_mutex.unlock();
}

cv::VideoCapture &VideoCaptureLoopback::operator>>(Mat &image) {
    frame_mutex.lock();
    cur_frame.assignTo(image);
    frame_mutex.unlock();
    return cam;
}

void VideoCaptureLoopback::release() {
    if (capturing_thread.joinable()) {
        keep_running = false;
        capturing_thread.join();
        cam.release();
        virtual_cam.release();
    }
}

void VirtualCamera::init_virtual_cam() const {
    struct v4l2_format video_format{};
    memset(&video_format, 0, sizeof(video_format));
    video_format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;

    if (ioctl(virtual_cam, VIDIOC_G_FMT, &video_format) < 0) {
        close(virtual_cam);
        cerr << "Warning: Unable to get video format from virtual camera" << endl;
    }

    video_format.fmt.pix.width = width;
    video_format.fmt.pix.height = height;
    video_format.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
    video_format.fmt.pix.sizeimage = width * height * 3;
    video_format.fmt.pix.field = V4L2_FIELD_NONE;

    if (ioctl(virtual_cam, VIDIOC_S_FMT, &video_format) < 0) {
        close(virtual_cam);
        cerr << "Warning: Unable to set video format of virtual camera" << endl;
    }
}

VirtualCamera::VirtualCamera(int virtual_index, int width, int height) {
    this->width = width;
    this->height = height;

    // Open virtual camera
    virtual_cam = open(("/dev/video" + to_string(virtual_index)).data(), O_RDWR);
    if(virtual_cam < 0) {
        cerr << "Warning: Cannot open virtual cam, check v4l2loopback settings" << endl;
    }

    // Initialize virtual camera
    init_virtual_cam();
}

bool VirtualCamera::write_frame(const cv::Mat& frame) const {
    Mat rgb_frame;
    cvtColor(frame, rgb_frame, COLOR_BGR2RGB);
    Mat resized_rgb_frame;
    resize(rgb_frame, resized_rgb_frame, Size(width, height));

    size_t written = write(virtual_cam, resized_rgb_frame.data, width * height * 3);
    if (written < 0) {
        close(virtual_cam);
        return false;
    }
    return true;
}

void VirtualCamera::release() const {
    close(virtual_cam);
}

VirtualCamera::VirtualCamera() = default;

bool VideoCaptureLoopback::isOpened() {
    return cam.isOpened();
}

VideoCaptureLoopback::VideoCaptureLoopback() = default;
